/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  ImuFactor.h
 *  @author Luca Carlone
 *  @author Stephen Williams
 *  @author Richard Roberts
 *  @author Vadim Indelman
 *  @author David Jensen
 *  @author Frank Dellaert
 **/

#pragma once

/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/base/debug.h>

namespace gtsam {

/*
 * If you are using the factor, please cite:
 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
 * conditionally independent sets in factor graphs: a unifying perspective based
 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
 *
 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
 * Robotics: Science and Systems (RSS), 2015.
 *
 * REFERENCES:
 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
 *     Volume 2, 2008.
 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
 *     High-Dynamic Motion in Built Environments Without Initial Conditions",
 *     TRO, 28(1):61-76, 2012.
 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
 *     Computation of the Jacobian Matrices", Tech. Report, 2013.
 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
 *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
 *     Robotics: Science and Systems (RSS), 2015.
 */

/**
 * PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
 * (rotation rates and accelerations) and the corresponding covariance matrix.
 * The measurements are then used to build the Preintegrated IMU factor.
 * Integration is done incrementally (ideally, one integrates the measurement
 * as soon as it is received from the IMU) so as to avoid costly integration
 * at time of factor construction.
 *
 * @addtogroup SLAM
 */
class PreintegratedImuMeasurements: public PreintegrationBase {

  friend class ImuFactor;
  friend class ImuFactor2;

protected:

  Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
  ///< (first-order propagation from *measurementCovariance*).

  /// Default constructor for serialization
  PreintegratedImuMeasurements() {
    preintMeasCov_.setZero();
  }

public:

 /**
   *  Constructor, initializes the class with no measurements
   *  @param bias Current estimate of acceleration and rotation rate biases
   *  @param p    Parameters, typically fixed in a single application
   */
  PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
      PreintegrationBase(p, biasHat) {
    preintMeasCov_.setZero();
  }

/**
  *  Construct preintegrated directly from members: base class and preintMeasCov
  *  @param base               PreintegrationBase instance
  *  @param preintMeasCov      Covariance matrix used in noise model.
  */
  PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
     : PreintegrationBase(base),
       preintMeasCov_(preintMeasCov) {
  }

  /// print
  void print(const std::string& s = "Preintegrated Measurements:") const;

  /// equals
  bool equals(const PreintegratedImuMeasurements& expected,
      double tol = 1e-9) const;

  /// Re-initialize PreintegratedIMUMeasurements
  void resetIntegration();

  /**
   * Add a single IMU measurement to the preintegration.
   * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
   * @param measuredOmega Measured angular velocity (as given by the sensor)
   * @param dt Time interval between this and the last IMU measurement
   */
  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt);

  /// Add multiple measurements, in matrix columns
  void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
                             const Matrix& dts);

  /// Return pre-integrated measurement covariance
  Matrix preintMeasCov() const { return preintMeasCov_; }

  /// Merge in a different set of measurements and update bias derivatives accordingly
  void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
  /// @deprecated constructor
  /// NOTE(frank): assumes Z-Down convention, only second order integration supported
  PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
      const Matrix3& measuredAccCovariance,
      const Matrix3& measuredOmegaCovariance,
      const Matrix3& integrationErrorCovariance,
      bool use2ndOrderIntegration = true);

  /// @deprecated version of integrateMeasurement with body_P_sensor
  /// Use parameters instead
  void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, double dt,
      boost::optional<Pose3> body_P_sensor);
#endif

private:

  /// Serialization function
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
    ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
  }
};

/**
 * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
 * the vehicle at previous time step), current state (pose and velocity at
 * current time step), and the bias estimate. Following the preintegration
 * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
 * are "summarized" using the PreintegratedIMUMeasurements class.
 * Note that this factor does not model "temporal consistency" of the biases
 * (which are usually slowly varying quantities), which is up to the caller.
 * See also CombinedImuFactor for a class that does this for you.
 *
 * @addtogroup SLAM
 */
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
    imuBias::ConstantBias> {
private:

  typedef ImuFactor This;
  typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
      imuBias::ConstantBias> Base;

  PreintegratedImuMeasurements _PIM_;

public:

  /** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
  typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif

  /** Default constructor - only use for serialization */
  ImuFactor() {}

  /**
   * Constructor
   * @param pose_i Previous pose key
   * @param vel_i  Previous velocity key
   * @param pose_j Current pose key
   * @param vel_j  Current velocity key
   * @param bias   Previous bias key
   */
  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedImuMeasurements& preintegratedMeasurements);

  virtual ~ImuFactor() {
  }

  /// @return a deep copy of this factor
  virtual gtsam::NonlinearFactor::shared_ptr clone() const;

  /// @name Testable
  /// @{
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const;
  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
  /// @}

  /** Access the preintegrated measurements. */

  const PreintegratedImuMeasurements& preintegratedMeasurements() const {
    return _PIM_;
  }

  /** implement functions needed to derive from Factor */

  /// vector of errors
  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
      const Pose3& pose_j, const Vector3& vel_j,
      const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
          boost::none, boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
          boost::none, boost::optional<Matrix&> H5 = boost::none) const;

  /// Merge two pre-integrated measurement classes
  static PreintegratedImuMeasurements Merge(
      const PreintegratedImuMeasurements& pim01,
      const PreintegratedImuMeasurements& pim12);

  /// Merge two factors
  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
  /// @deprecated typename
  typedef PreintegratedImuMeasurements PreintegratedMeasurements;

  /// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams
  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedMeasurements& preintegratedMeasurements,
      const Vector3& n_gravity, const Vector3& omegaCoriolis,
      const boost::optional<Pose3>& body_P_sensor = boost::none,
      const bool use2ndOrderCoriolis = false);

  /// @deprecated use PreintegrationBase::predict,
  /// in the new one gravity, coriolis settings are in PreintegrationParams
  static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
      Vector3& vel_j, const imuBias::ConstantBias& bias_i,
      PreintegratedMeasurements& pim, const Vector3& n_gravity,
      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
#endif

private:

  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NoiseModelFactor5",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(_PIM_);
  }
};
// class ImuFactor

/**
 * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
 * @addtogroup SLAM
 */
class ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
private:

  typedef ImuFactor2 This;
  typedef NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> Base;

  PreintegratedImuMeasurements _PIM_;

public:

  /** Default constructor - only use for serialization */
  ImuFactor2() {}

  /**
   * Constructor
   * @param state_i Previous state key
   * @param state_j Current state key
   * @param bias    Previous bias key
   */
  ImuFactor2(Key state_i, Key state_j, Key bias,
             const PreintegratedImuMeasurements& preintegratedMeasurements);

  virtual ~ImuFactor2() {
  }

  /// @return a deep copy of this factor
  virtual gtsam::NonlinearFactor::shared_ptr clone() const;

  /// @name Testable
  /// @{
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const;
  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
  /// @}

  /** Access the preintegrated measurements. */

  const PreintegratedImuMeasurements& preintegratedMeasurements() const {
    return _PIM_;
  }

  /** implement functions needed to derive from Factor */

  /// vector of errors
  Vector evaluateError(const NavState& state_i, const NavState& state_j,
                       const imuBias::ConstantBias& bias_i,  //
                       boost::optional<Matrix&> H1 = boost::none,
                       boost::optional<Matrix&> H2 = boost::none,
                       boost::optional<Matrix&> H3 = boost::none) const;

private:

  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NoiseModelFactor3",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(_PIM_);
  }
};
// class ImuFactor2

template <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};

template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {};

template <>
struct traits<ImuFactor2> : public Testable<ImuFactor2> {};

} /// namespace gtsam
